#include "robohub.h"

#include <string.h>
#include <thread>
#include <chrono>
#include <string>

#include "rtdev/rtdev.h"
#include "rtbus/rtbus.h"
#include "jkutil/configparser.h"
#include "jkutil/bitdataop.h"
#include "log/zlog.hh"

#define ROBOHUB_ERR(fmt, ...) zlog()->error("[RoboHub] " fmt, ##__VA_ARGS__)
#define ROBOHUB_INFO(fmt, ...) zlog()->info("[RoboHub] " fmt, ##__VA_ARGS__)

using rtdev::HubRobotPower;
using rtdev::RoboHubCab;
using rtdev::RoboHubSafetySignal;
using rtdev::RoboHubScb;
using rtdev::SafetyBoardItf;

static const uint32_t kControlWordIndex = 0x00200000;
static const uint32_t kControlWordDisplayIndex = 0x00200100;
static const uint32_t kStateWordIndex = 0x00200200;
// static const uint32_t kUserDOIndex = 0x00630001;
// static const uint32_t kUserDOFeedbackIndex = 0x00630301;
// static const uint32_t kSafetyDIIndex = 0x00610002;
static const uint32_t kHubScbVendorId = 0x661100;
static const uint32_t kHubScbProductId = 0x00000001;  //FIXME:需要和其他类型的区分开
static const uint32_t kHubScbRevisionNumber = 0x00000001;

HubRobotPower::HubRobotPower()
{
    feedbacks.fill(Feedback());
    scb = nullptr;
}

int HubRobotPower::set_power(bool power, int robid)
{
    if (robid >= (int)feedbacks.size())
        return -1;
    if (robid < 0)
    {
        for (auto& fb : feedbacks) { fb.is_cmd_power_on = power; }
    }
    else
    {
        feedbacks[robid].is_cmd_power_on = power;
    }
    return 0;
}
double HubRobotPower::get_power(int robid)
{
    if (robid >= (int)feedbacks.size() || robid < 0)
        return -1;
    if (scb->is_sim())
    {
        return 0;
    }
    return feedbacks[robid].power;
}
double HubRobotPower::get_voltage(int robid)
{
    if (robid >= (int)feedbacks.size() || robid < 0)
        return -1;
    if (scb->is_sim())
    {
        return 0;
    }
    return feedbacks[robid].voltage;
}
double HubRobotPower::get_current(int robid)
{
    if (robid >= (int)feedbacks.size() || robid < 0)
        return -1;
    if (scb->is_sim())
    {
        return 0;
    }
    return feedbacks[robid].current;
}

RoboHubScb::RoboHubScb()
{
    bus_id_ = 0;
    bus2_id_ = -1;

    sim_flag_ = 0;
    cab_.scb = this;
    rob.scb = this;

    last_sword = 0;
}
const char* RoboHubScb::name() { return "ZYSCB"; }
int RoboHubScb::init(void* info, int sim)
{
    //根据配置信息找到总线
    auto bus = rtbus::BusMgr::get()->bus("scbbus");
    if (bus.get() == nullptr)
    {
        return -1;
    }
    bus_ = bus;

    sim_flag_ = sim;
    ROBOHUB_INFO("init success.bus:{} sim:{}", bus_->get_name(), is_sim());
    return 0;
}

int RoboHubScb::boot()
{
    if (!is_sim())
    {
        //配置从站
        rtbus::RtCommuItf::SlaveInfo slave;
        slave.slaveid = 0;
        slave.cycletime_us = 8000;
        slave.shifttime_us = 4000;
        slave.vendor_id = kHubScbVendorId;
        slave.product_code = kHubScbProductId;
        slave.revision_number = kHubScbRevisionNumber;
        slave.enable_dc = 0;  //scb采用sm通讯，不需要dc
        bus_->config_slave(slave);

        //配置pdo通道
        bus_->config_txpdo(kControlWordIndex, 4);  // control word
        // bus_->config_txpdo(kUserDOIndex, 2);              // user DO
        bus_->config_rxpdo(kControlWordDisplayIndex, 4);  // Control Word Display
        bus_->config_rxpdo(kStateWordIndex, 4);           // State Word
        // bus_->config_rxpdo(kSafetyDIIndex, 2);            // Safety DI
        // bus_->config_rxpdo(kUserDOFeedbackIndex, 2);      // user DO feedback

        //启动总线
        if (bus_->active())
        {
            ROBOHUB_ERR("bus {} active failed.", bus_->get_name());
            return -2;
        }

#if 0
        //读取硬件版本号
        const int retry_cnt = 100;
        uint16_t hw_type = 0, hw_ver = 0;
        int err = get_sdo_blocking(0x210201, &hw_type, 2, retry_cnt);
        if (err)
        {
            return err;
        }
        get_sdo_blocking(0x210202, &hw_ver, 2, retry_cnt);
        HSCB_INFO("hw type: {}, hw ver: {}", hw_type, hw_ver);

        //读取软件版本号
        uint32_t sw_ver = 0;
        get_sdo_blocking(0x210205, &sw_ver, 4, retry_cnt);
#else
        uint32_t cwdis = 0;
        get_sdo_blocking(kControlWordDisplayIndex, &cwdis, 4);
#endif
    }
    return 0;
}

int RoboHubScb::set_send_data(int busid)
{
    check_alive();
    if (!bus_)
        return -1;

    uint32_t controlword = 0;

    //此数据参看HSCB文档
    Util::set_bit((uint8_t*)&controlword, 0, cab_.cmd.dis_robot_commu);          // 请求伺服驱动刷机
    Util::set_bit((uint8_t*)&controlword, 1, rob.feedbacks[0].is_cmd_power_on);  // 机器人本体1电源控制
    //FIXME: 机器人本体2电源控制固件暂未支持
    Util::set_bit((uint8_t*)&controlword, 2, 0);  //FIXME: 关闭IPC电源。1 表示控制器做好关机准备，允许SCB切断IPC电源并关机。
    Util::set_bit((uint8_t*)&controlword, 3, 0);
    Util::set_bit((uint8_t*)&controlword, 4, 0);
    Util::set_bit((uint8_t*)&controlword, 5, 0);
    Util::set_bit((uint8_t*)&controlword, 6, 0);
    Util::set_bit((uint8_t*)&controlword, 7, 0);
    bus_->set_txpdo_data(kControlWordIndex, (uint8_t*)&controlword);
    return 0;
}
int RoboHubScb::get_recv_data(int busid)
{
    if (!bus_)
        return -1;
    uint32_t cwdis = 0;
    auto alive_cnt = bus_->get_rxpdo_data(kControlWordDisplayIndex, (uint8_t*)&cwdis);
    if (keep_alive(alive_cnt))
    {
        return -1;
    }
    uint32_t sword = 0;
    bus_->get_rxpdo_data(kStateWordIndex, (uint8_t*)&sword);
    if (last_sword != sword)
    {
        ROBOHUB_INFO("stateword change from {} to {}", last_sword, sword);
        last_sword = sword;
    }
    // bit0: 所有急停信号的汇总，目前包括：面板急停，外部输入急停.1表示有急停信号，0表示无急停信号
    safedev.ei[0] = Util::get_bit((uint8_t*)&sword, 0) == 1 ? 0 : 1;
    safedev.ei[1] = Util::get_bit((uint8_t*)&sword, 0) == 1 ? 0 : 1;
    safedev.ei_fault = false;

    // bit1:所有保护性停止信号的汇总，目前包括：外部输入保护性停止.1表示有保护性停止信号，0表示无保护性停止信号
    // safedev.si[0] = Util::get_bit((uint8_t*)&sword, 1) == 1 ? 0 : 1;
    // safedev.si[1] = Util::get_bit((uint8_t*)&sword, 1) == 1 ? 0 : 1;
    // safedev.si_fault = false;

    //FIXME: 其他暂未支持
    return 0;
}
int RoboHubScb::get_sdo_blocking(uint32_t objid, void* data, int s, int retry) { return bus_->get_sdo_blocking(0x12, objid, data, s, retry); }
int RoboHubScb::set_sdo_blocking(uint32_t objid, void* data, size_t s, void* readback, int retry)
{
    return bus_->set_sdo_blocking(0x12, objid, data, s, readback, retry);
}

RoboHubCab::RoboHubCab()
{
    cmd_cab_id = act_cab_id = 0;
    cmd_stuck_voltage = act_stuck_voltage = 58;
    scb = nullptr;
}

int RoboHubCab::get_id()
{
    if (scb->is_sim())
    {
        return 0;
    }
    return act_cab_id;
}

int RoboHubCab::set_id(uint32_t id)
{
    if (scb->is_sim())
        return 0;
    cmd_cab_id = id;
    return scb->set_sdo_blocking(0x314e00, &id, 4, &act_cab_id, 3);
}

int RoboHubCab::get_stuck_voltage(bool is_act) { return is_act ? act_stuck_voltage : cmd_stuck_voltage; }

int RoboHubCab::set_stuck_voltage(int voltage)
{
    // FIXME:RoboHubScb固件不支持
    return 0;
}

int RoboHubCab::set_power_supply(int val)
{
    // FIXME:RoboHubScb固件不支持
    return -1;
}

int RoboHubCab::get_power_supply()
{
    // FIXME:RoboHubScb固件不支持
    return 48;
}

RoboHubSafetySignal::RoboHubSafetySignal()
{
    ei[0] = ei[1] = true;
    ei_fault = false;
    ext_ei[0] = ext_ei[1] = true;
    ext_ei_fault = false;
    si[0] = si[1] = true;
    si_fault = false;
}
int RoboHubSafetySignal::get_ei_stat(bool dual_chn[2], bool* has_fault)
{
    dual_chn[0] = ei[0];
    dual_chn[1] = ei[1];
    if (has_fault)
        *has_fault = ei_fault;
    return 0;
}
int RoboHubSafetySignal::get_ext_ei_stat(bool dual_chn[2], bool* has_fault)
{
    dual_chn[0] = ext_ei[0];
    dual_chn[1] = ext_ei[1];
    if (has_fault)
        *has_fault = ext_ei_fault;
    return 0;
}
int RoboHubSafetySignal::get_si_stat(bool dual_chn[2], bool* has_fault)
{
    dual_chn[0] = si[0];
    dual_chn[1] = si[1];
    if (has_fault)
        *has_fault = si_fault;
    return 0;
}
int RoboHubSafetySignal::set_tcp_offset(float x, float y, float z) { return 0; }